#include <iostream>
#include <map>
#include <list>
#include <string>
#include <chrono>
#include <thread>
#include "easylogging++.h"
#include <libsumo/libsumo.h>
#include <RTI.hh>
#include <fedtime.hh>
#include <iomanip>
#include "hwFederateAmbassador/HwFederateAmbassador.hh"
#include "SumoRti.h"
#include "Utils.h"

using namespace std;
using namespace libsumo;

RTI::RTIambassador rtiAll;
RTI::ObjectHandle netOffsetAll;

RTI::InteractionClassHandle hSumoClass;

RTI::InteractionClassHandle hInfoHead;
RTI::ParameterHandle hVehNum;
RTI::ParameterHandle hDeparted;
RTI::ParameterHandle hArrived;

RTIfedTime g_currentTime = 0.0; // 当前的时间
bool g_bConstrained = false;    // 启用时间限制。RTI同意将仿真成员设置为时间管理受限的成员
bool g_bRegulation = false;     // 启用时间调节。RTI同意将仿真成员设置为时间管控成员
bool g_granted = false;         // 推进下一步标志位

SumoArguments sumoArguments;

int sumo_rti(int argc, char *argv[]);

int sumo_rti(int argc, char *argv[]) {
    RTIHelper rtiHelper("Co-simulation", "Co-simulation-8.14.fed", "Sumo");
    sumoArguments.Print();

    LOG(INFO) << "filePath: " << sumoArguments.filePath;

    double simulationTime = 3600;                     // 秒
    int waitTime = (int) (sumoArguments.sleep * 1000); // 毫秒
    SumoRtiHelper sumoHelper(sumoArguments);

    int step = 0;

    LOG(INFO) << "...sumo运行......";

    try {
        RTI::RTIambassador rti;

        RTIambassadorReturn rtiReturn = rtiHelper.createFederationExecution();
        if (rtiReturn.errorID)
            return rtiReturn.errorID;
        rti = rtiReturn.rti;
        rtiAll = rti;

        if (rtiHelper.joinFederationExecution().errorID)
            return rtiHelper.joinFederationExecution().errorID;

        // 获取参数，准备设置参数
        hInfoHead = rti.getInteractionClassHandle("info_head");
        hVehNum = rti.getParameterHandle("vehNum", hInfoHead);
        hDeparted = rti.getParameterHandle("departed", hInfoHead);
        hArrived = rti.getParameterHandle("arrived", hInfoHead);

        hSumoClass = rti.getInteractionClassHandle("sumo_car");
        map<string, RTI::ParameterHandle> hSumoList = sumoHelper.getParameterHandle(rtiReturn, hSumoClass);

        // 如果向外发送，则需要公布
        rti.publishInteractionClass(hSumoClass);
        rti.publishInteractionClass(hInfoHead);

        netOffsetAll = sumoHelper.processingNetworkOffset(rti, sumoArguments);

        rti.subscribeInteractionClass(hInfoHead);

        // 将仿真成员设置为时间管理受限的
        rti.enableTimeConstrained();
        LOG(TRACE) << " 将仿真成员设置为时间管理受限的";
        LOG(TRACE) << " 等待RTI同意将该仿真成员设置为 时间管理受限的......";
        while (!g_bConstrained) {
            rti.tick(0.001, 1.0);
        }

        RTIfedTime lookahead = 0.0001; //
        rti.enableTimeRegulation((RTIfedTime) 0.1, lookahead);
        LOG(TRACE) << " 将仿真成员设置为时间管控成员";
        LOG(TRACE) << " 等待RTI同意将该仿真成员设置为 时间管控成员......";
        while (!g_bRegulation) {
            rti.tick(0.001, 1.0);
        }

        // 打开异步消息开关
        rti.enableAsynchronousDelivery();
        LOG(TRACE) << " 打开异步消息开关" << endl;

        RTIfedTime intervalTime = sumoArguments.stepLength; // 仿真周期设置为1秒；这里的逻辑时间1对应物理时间的1秒
        // （假设设置为2，则1个逻辑时间单位对应物理时间的0.5秒，
        // 2个逻辑时间单位对应仿真周期1秒）；

        while (step < simulationTime / sumoArguments.stepLength - 1) {
            LOG(INFO) << "Step: " << step << ", SumoTime: " << Simulation::getTime() << ", 仿真时间："
                      << std::fixed << std::setprecision(2) << g_currentTime.getTime(); // 单位：s
            LOG(TRACE) << (Utils::Number::lessThan(Simulation::getTime(), g_currentTime) ? "需要Sumo单独推进"
                                                                                         : "不需要Sumo单独推进");
            if (Utils::Number::lessThan(Simulation::getTime(), g_currentTime)) {
                LOG(TRACE) << "Sumo单独推进";
                Simulation::step();
                step++;
                continue;
            }

            // 订阅新加入的车辆的信息
            auto departedIdList = Simulation::getDepartedIDList();
            string departIdListStr;
            for (const auto &id: departedIdList) {
                cout << id << ", ";
            }
            cout << endl;
            for (const auto &id: departedIdList) {
                Vehicle::subscribe(id, {
                        VAR_TYPE, VAR_VEHICLECLASS, VAR_COLOR,
                        VAR_LENGTH, VAR_WIDTH, VAR_HEIGHT,
                        VAR_POSITION3D,
                        VAR_ANGLE, VAR_SLOPE,
                        VAR_SPEED, VAR_SPEED_LAT,
                        VAR_SIGNALS});
                departIdListStr.append(id).append(",");
            }
            LOG(INFO) << "新加入车：" << departIdListStr;

            //取消订阅已经到达的车辆
            auto arrivedIdList = Simulation::getArrivedIDList();
            string arrivedIdListStr;
            for (const auto &id: arrivedIdList) {
                Vehicle::unsubscribe(id);
                arrivedIdListStr.append(id).append(",");
            }

            // 所有订阅的车辆
            auto allSubscriptionResults = Vehicle::getAllSubscriptionResults();
            // 车辆数量
            RTI::ParameterHandleValuePairSet *infoHeadParams;
            infoHeadParams = RTI::ParameterSetFactory::create(3);
            infoHeadParams->add(hVehNum, (char *) to_string(allSubscriptionResults.size()).c_str(),
                                to_string(allSubscriptionResults.size()).size());
            infoHeadParams->add(hDeparted, departIdListStr.c_str(), departIdListStr.size());
            infoHeadParams->add(hArrived, arrivedIdListStr.c_str(), arrivedIdListStr.size());
            rti.sendInteraction(hInfoHead, *infoHeadParams, "info_head");
            infoHeadParams->empty();
            delete infoHeadParams;

            sumoHelper.sendSumoVehInteraction(rti, allSubscriptionResults, hSumoList, hSumoClass);

            // 推进联邦仿真
            try {
//                RTIfedTime targetTime = g_currentTime + intervalTime;
                RTIfedTime targetTime = g_currentTime + intervalTime;
                LOG(TRACE) << "Sumo请求将仿真推进到: " << targetTime << "秒";
                rti.timeAdvanceRequest(targetTime);
                LOG(TRACE) << "Sumo等待RTI同意该仿真成员推进到下一步......";
                while (!g_granted) {
                    rti.tick(0.001, 1.0);
                    //LOG(TRACE) <<"等待同意";
                }
                g_granted = false;
                Simulation::step();
                LOG(TRACE) << "Sumo已经仿真到：" << Simulation::getTime() << ", SumoRTI已经推进到: " << g_currentTime
                           << "秒 "
                           << endl;
                step++;
            }
            catch (RTI::Exception &e) {
                LOG(ERROR) << "Error: " << e << endl;
                return 406;
            }
            this_thread::sleep_for(chrono::milliseconds(waitTime));
        }

        rtiHelper.resignFederationExecution();
        rtiHelper.destroyFederationExecution();
    }
    catch (RTI::ConcurrentAccessAttempted &e) {
        LOG(ERROR) << "Error: " << e << endl;
        return 410;
    }
    catch (RTI::Exception &e) {
        cerr << "FED_HW: ERROR:" << e << endl;
        return 411;
    }
    return 0;
}

int main(int argc, char *argv[]) {
    logConfig(argc, argv);
    sumoArguments = parameterAnalysis(argc, argv);
    return sumo_rti(argc, argv);
}
